/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "fc2d_Solvers.h"  // for fc2d_projc

void fc2d_projc(double xi[], int *nn, int statusi[], double pi[], double fric[],
                double *projc1, int *projc2) {
  int i, nc, n = *nn, stat;

  double mu1;

  nc = n / 2;

  for (i = 0; i < nc; i++) {
    mu1 = fric[i];
    stat = statusi[i];

    if (xi[2 * i] <= 0.0) /* No contact status  */
    {
      projc1[2 * i] = 0.0;
      projc1[2 * i + 1] = 0.0;
      projc2[i] = 0;

    } else {
      projc1[2 * i] = xi[2 * i];

      if (xi[2 * i + 1] <= -mu1 * xi[2 * i]) /*  Slide backward     */
      {
        projc1[2 * i + 1] = -mu1 * xi[2 * i];
        projc2[i] = 1;

      } else if (xi[2 * i + 1] >= mu1 * xi[2 * i]) /*  Slide forward      */
      {
        projc1[2 * i + 1] = mu1 * xi[2 * i];
        projc2[i] = 3;
      } else {
        if (pi[2 * i + 1] == 0.0) {
          if (stat == 1) /*  Slide backward     */
          {
            projc1[2 * i + 1] = -mu1 * xi[2 * i];
            projc2[i] = 1;
          } else if (stat == 3) /*  Slide forward        */
          {
            projc1[2 * i + 1] = mu1 * xi[2 * i];
            projc2[i] = 3;
          } else
          /*   Stick contact        */
          {
            projc1[2 * i + 1] = xi[2 * i + 1];
            projc2[i] = 2;
          }
        } else
        /*   Stick contact      */
        {
          projc1[2 * i + 1] = xi[2 * i + 1];
          projc2[i] = 2;
        }
      }
    }
  }
}
